/*
 * Aircraft.cpp
 *
 *  Created on: Apr 28, 2011
 *      Author: tflanzer
 */

#include "Aircraft.h"
#include <string.h>
#include <math.h>
using namespace std;

// Define constants for use in BLAS routines
const int izero = 0;
const int ione = 1;
const int ithree = 3;
const double dzero = 0;
const double done = 1;
const int ifive = 5.0;
const int ieight = 8.0;

Aircraft::Aircraft() {
	geomTot gt;
	colTot ct;

	// Set up geometry
	geomSetup(gt);

	// Set up lattice
	latticeSetup(ct,gt);

	// Initialize aircraft parameters
	acInit(ac,ct);

	// Initialize environmental parameters
	envInit(en);

	// Set up atmosphere
	atm = new SeaLevelAtmosphere;

	// Load stability derivative matrix
	loadSDMatrix();

	// Set up electric propulsion
//	m = new Motor(8.0, 3691.0, 0.28, 1.04, 4);
//	double r = 1.5;
//	p = new Propeller(2, r, 0.18/r, .435/r, .405/r, 20, 11, 0.03, 0, 0.01, 0.1, 1);
	propulsion = new ElectricPropulsion(atm);
}

Aircraft::~Aircraft() {
}

void Aircraft::geomSetup(geomTot &gt) {

	if(strcmp(AIRCRAFT_TYPE, "EASYGLIDERPRO") == 0) {

		/* THRUST HACK FOR NOW */
		ac.maxThrust = 1;

		/* MAIN WING */

		gt.gelem[0].N = 21; // Total number of panels
		gt.gelem[0].Ns = 3; // Total number of spanwise section

		gt.gelem[0].yle[0] = 0.335; // Span projected onto y-axis
		gt.gelem[0].yle[1] = 0.745;
		gt.gelem[0].yle[2] = 0.920;

		gt.gelem[0].zle[0] = -0.0117; // Z leading edge height
		gt.gelem[0].zle[1] = -0.0260;
		gt.gelem[0].zle[2] = -0.0321;

		gt.gelem[0].chord[0] = 0.200; // Chord
		gt.gelem[0].chord[1] = 0.200;
		gt.gelem[0].chord[2] = 0.165;
		gt.gelem[0].chord[3] = 0.090;

		gt.gelem[0].sweep[0] = 0; // Sweep
		gt.gelem[0].sweep[1] = 0;
		gt.gelem[0].sweep[2] = 0;

		gt.gelem[0].twist[0] = 3.0*PI/180.0; // Twist
		gt.gelem[0].twist[1] = 3.0*PI/180.0;
		gt.gelem[0].twist[2] = 2.0*PI/180.0;
		gt.gelem[0].twist[3] = 1.0*PI/180.0;

		gt.gelem[0].apex[0] = 0; // Apex
		gt.gelem[0].apex[1] = 0;
		gt.gelem[0].apex[2] = 0;

		gt.gelem[0].mirror = 1; // Mirror geometry

		gt.gelem[0].cs[0] = 0; // Control ID (0 = none, 1 = ail, 2 = ele,
		gt.gelem[0].cs[1] = 1; // 			  3 = rud,  4 = flap)
		gt.gelem[0].cs[2] = 0;

		/* HORIZONTAL TAIL */

		gt.gelem[1].N  = 10; // Total number of panels
		gt.gelem[1].Ns = 1; // Total number of spanwise section
		gt.gelem[1].yle[0] = 0.235; // Span projected onto y-axis
		gt.gelem[1].zle[0] = 0.0;  // Z leading edge height
		gt.gelem[1].chord[0] = 0.105; // Chord
		gt.gelem[1].chord[1] = 0.050;
		gt.gelem[1].sweep[0] = 0; // Sweep
		gt.gelem[1].twist[0] = 0.0*PI/180.0; // Twist
		gt.gelem[1].twist[1] = 0.0*PI/180.0;
		gt.gelem[1].apex[0] = -0.62625; // Apex
		gt.gelem[1].apex[1] = 0;
		gt.gelem[1].apex[2] = 0;
		gt.gelem[1].mirror = 1; // Mirror geometry
		gt.gelem[1].cs[0] = 2; // Control ID (0 = none, 1 = ail, 2 = ele,
							   // 			  3 = rud,  4 = flap)

		/* VERTICAL TAIL */

		gt.gelem[2].N  = 20; // Total number of panels
		gt.gelem[2].Ns = 1; // Total number of spanwise section
		gt.gelem[2].yle[0] = 0.0; // Span projected onto y-axis
		gt.gelem[2].zle[0] = -0.29;  // Z leading edge height
		gt.gelem[2].chord[0] = 0.245; // Chord
		gt.gelem[2].chord[1] = 0.075;
		gt.gelem[2].sweep[0] = 20.0*PI/180.0; // Sweep
		gt.gelem[2].twist[0] = 0.0*PI/180.0; // Twist
		gt.gelem[2].twist[1] = 0.0*PI/180.0;
		gt.gelem[2].apex[0] = -0.62625; // Apex
		gt.gelem[2].apex[1] = 0;
		gt.gelem[2].apex[2] = 0.050;
		gt.gelem[2].mirror = 0; // Mirror geometry
		gt.gelem[2].cs[0] = 3; // Control ID (0 = none, 1 = ail, 2 = ele,
							   // 			  3 = rud,  4 = flap)
	}
}

void Aircraft::aeroSolverVLM(double *state, double *ctrl) {

	// Local variables
	double V, alpha, beta, q, winfv[3], tmp[3], Ft[3], Fw[3],
    	   Mt[3], *pnl, rho;
	double *bc, *gam, *winf, *wr, *wt, *ui, *vi, *wi, *G, *Fp, *Fn, *Mn, *dcm, *we;
	int Ntot, i;

	// Allocate memory
	Ntot = ac.Ntot;
	bc   = (double*)malloc(sizeof(double)*Ntot);
	gam  = (double*)malloc(sizeof(double)*Ntot);
	winf = (double*)malloc(sizeof(double)*Ntot*3);
	wr   = (double*)malloc(sizeof(double)*Ntot*3);
	wt   = (double*)malloc(sizeof(double)*Ntot*3);
	we   = (double*)malloc(sizeof(double)*Ntot*3);
	ui   = (double*)malloc(sizeof(double)*Ntot);
	vi   = (double*)malloc(sizeof(double)*Ntot);
	wi   = (double*)malloc(sizeof(double)*Ntot);
	G    = (double*)malloc(sizeof(double)*Ntot*3);
	Fp   = (double*)malloc(sizeof(double)*Ntot*3);
	Fn   = (double*)malloc(sizeof(double)*Ntot*3);
	Mn   = (double*)malloc(sizeof(double)*Ntot*3);
	pnl  = (double*)malloc(sizeof(double)*Ntot*3);
	dcm  = (double*)malloc(sizeof(double)*9);

	// Get state variables
	double D  = *(state+2);
	double VNi = *(state+3);
	double VEi = *(state+4);
	double VDi = *(state+5);
	double q0 = *(state+6);
	double q1 = *(state+7);
	double q2 = *(state+8);
	double q3 = *(state+9);
	double pb = *(state+10);
	double qb = *(state+11);
	double rb = *(state+12);
	double VNw = *(state+13);
	double VEw = *(state+14);
	double VDw = *(state+15);

	// Get control inputs
	double ail = *(ctrl+0);
	double ele = *(ctrl+1);
	double rud = *(ctrl+2);
	double flp = 0;
	double thr = *(ctrl+4);

	// Compute Euler angles
	double phi = atan2(2*(q0*q1+q2*q3),1-2*(q1*q1+q2*q2));
	double th  = asin(2*(q0*q2-q3*q1));
	double psi = atan2(2*(q0*q3+q1*q2),1-2*(q2*q2+q3*q3));

	// Compute DCM matrix to convert body velocities to inerital velocities
	body2inertial(psi,th,phi,dcm);
	double ub = *(dcm)*VNi   + *(dcm+1)*VEi + *(dcm+2)*VDi;
	double vb = *(dcm+3)*VNi + *(dcm+4)*VEi + *(dcm+5)*VDi;
	double wb = *(dcm+6)*VNi + *(dcm+7)*VEi + *(dcm+8)*VDi;

	// Compute V, alpha, beta
	V = sqrt(pow(ub,2)+pow(vb,2)+pow(wb,2));
	alpha = atan(wb/(ub));
	beta = asin(vb/V);

	// Compute dynamic pressure
	rho = 1.225; //atm->getDensity(D);
	q = 0.5*rho*pow(V,2);

	// Compute free stream winds
	winfv[0] = V*cos(alpha)*cos(beta);
	winfv[1] = V*sin(beta);
	winfv[2] = V*sin(alpha)*cos(beta);
	for (i = 0; i < Ntot; i++) {
		*(winf+i)        = winfv[0];
		*(winf+i+Ntot)   = winfv[1];
		*(winf+i+2*Ntot) = winfv[2];
	}

	// Get environmental winds
	envwinds(state, dcm, we);

	// Compute winds due to rotation at the 3/4 chord point
	tmp[0] = 0; tmp[1] = 0; tmp[2] = 0;
	for (i = 0; i < Ntot; i++) {
		crossprod(pb,qb,rb,ac.macp[i][0],ac.macp[i][1],ac.macp[i][2],tmp);
		*(wr+i)        = tmp[0];
		*(wr+i+Ntot)   = tmp[1];
		*(wr+i+2*Ntot) = tmp[2];
	}

	// Compute total winds at 3/4 chord point
	for (i = 0; i < Ntot; i++) {
		*(wt+i)        = *(winf+i)        + *(wr+i)       - *(we+i);
		*(wt+i+Ntot)   = *(winf+i+Ntot)   + *(wr+i+Ntot)  - *(we+i+Ntot);
		*(wt+i+2*Ntot) = *(winf+i+2*Ntot) + *(wr+i+2*Ntot)- *(we+i+2*Ntot);
	}

	// Compute control surface impact
	for (int j = 0; j < 3; j++) {
		for (int i = 0; i < Ntot; i++) {
			*(pnl+i+j*Ntot) = ac.pnorm[i][j];
		}
	}
	controlSurfaces(Ntot,ail,ele,rud,flp,pnl,ac.cs,ac.tw,ac.di);

	// Compute boundary condition
	for (i = 0; i < Ntot; i++) {
		bc[i] = -(*(pnl+i)*(*(wt+i))+(*(pnl+i+Ntot))*(*(wt+i+Ntot))+
				(*(pnl+i+2*Ntot))*(*(wt+i+2*Ntot)));
	}

	// Compute gamma
	dgemv_("n",&Ntot,&Ntot,&done,ac.AICinv,&Ntot,bc,&ione,&dzero,gam,&ione);

	// Compute uinduced, vinduced, winduced at center of bound votex
	dgemv_("n",&Ntot,&Ntot,&done,ac.udw,&Ntot,gam,&ione,&dzero,ui,&ione);
	dgemv_("n",&Ntot,&Ntot,&done,ac.vdw,&Ntot,gam,&ione,&dzero,vi,&ione);
	dgemv_("n",&Ntot,&Ntot,&done,ac.wdw,&Ntot,gam,&ione,&dzero,wi,&ione);

	// Compute winds due to rotation at the 1/4 chord point
	for (i = 0; i < Ntot; i++) {
		crossprod(pb,qb,rb,ac.maqc[i][0],ac.maqc[i][1],ac.maqc[i][2],tmp);
		*(wr+i)        = tmp[0];
		*(wr+i+Ntot)   = tmp[1];
		*(wr+i+2*Ntot) = tmp[2];
	}

	// Compute total winds at center of bound vortex
	for (i = 0; i < Ntot; i++) {
		*(wt+i)        = *(winf+i)        + *(ui+i) + *(wr+i)        - *(we+i);;
		*(wt+i+Ntot)   = *(winf+i+Ntot)   + *(vi+i) + *(wr+i+Ntot)   - *(we+i+Ntot);
		*(wt+i+2*Ntot) = *(winf+i+2*Ntot) + *(wi+i) + *(wr+i+2*Ntot) - *(we+i+2*Ntot);
	}

	// Project gamma along bound vortex
	for (i = 0; i < Ntot; i++) {
		*(G+i)        = *(gam+i)*ac.gvec[i][0];
		*(G+i+Ntot)   = *(gam+i)*ac.gvec[i][1];
		*(G+i+2*Ntot) = *(gam+i)*ac.gvec[i][2];
	}

	// From KJ theorem find forces
	for (i = 0; i < Ntot; i++) {
		crossprod(*(wt+i),*(wt+i+Ntot),*(wt+i+2*Ntot),*(G+i),*(G+i+Ntot),*(G+i+2*Ntot),tmp);
		*(Fp+i)        = rho*tmp[0];
		*(Fp+i+Ntot)   = rho*tmp[1];
		*(Fp+i+2*Ntot) = rho*tmp[2];
	}

	// Compute force per panel
	for (i = 0; i < Ntot; i++) {
		*(Fn+i)        = *(Fp+i)*ac.plt[i];
		*(Fn+i+Ntot)   = *(Fp+i+Ntot)*ac.plt[i];
		*(Fn+i+2*Ntot) = *(Fp+i+2*Ntot)*ac.plt[i];
	}

	// Compute body to wind DCM
	bodyTOwind(alpha,beta,dcm);

	// Check CLmax constraint
	checkCLmax(Ntot, dcm, Fn, rho, wt, ac.pa, ac.cs, ac.CLmax, ac.CLmin, flp);

	// Compute total force in the body frame
	*Ft = 0; *(Ft+1) = 0; *(Ft+2) = 0;
	for (i = 0; i < Ntot; i++) {
		*Ft     += *(Fn+i);
		*(Ft+1) += *(Fn+i+Ntot);
		*(Ft+2) += *(Fn+i+2*Ntot);
	}

	// Convert from body frame to wind frame
	dgemv_("n",&ithree,&ithree,&done,dcm,&ithree,Ft,&ione,&dzero,Fw,&ione);

	// Add viscous drag
	*Fw += ac.CDp*(q*ac.Sref);

	// Convert from wind frame to body frame
	dgemv_("t",&ithree,&ithree,&done,dcm,&ithree,Fw,&ione,&dzero,Ft,&ione);

	// Compute moments per panel
	for (i = 0; i < Ntot; i++) {
		crossprod(ac.maqc[i][0],ac.maqc[i][1],ac.maqc[i][2],*(Fn+i),*(Fn+i+Ntot),*(Fn+i+2*Ntot),tmp);
		*(Mn+i)        = tmp[0];
		*(Mn+i+Ntot)   = tmp[1];
		*(Mn+i+2*Ntot) = tmp[2];
	}

	// Compute total moments
	*Mt = 0; *(Mt+1) = 0; *(Mt+2) = 0;
	for (i = 0; i < Ntot; i++) {
		*Mt     += *(Mn+i);
		*(Mt+1) += *(Mn+i+Ntot);
		*(Mt+2) += *(Mn+i+2*Ntot);
	}

	// Update pitching moment to include airfoil pitching moment
	*(Mt+1) += ac.Cm0 * (q * ac.Sref * ac.cref);

	// Handle numerical issues
	for (i = 0; i < 3; i++) {
		if (fabs(*(Mt+i)) < 1e-9) {
			*(Mt+i) = 0;
		}
		if (fabs(*(Ft+i)) < 1e-9) {
			*(Ft+i) = 0;
		}
	}

	// Store forces
	as.F[0]     = *Ft;
	as.F[1] = *(Ft+1);
	as.F[2] = *(Ft+2);

	// Store moments
	as.M[0]     = *Mt;
	as.M[1] = *(Mt+1);
	as.M[2] = *(Mt+2);

	// Write coefficients
	as.CD     = *Fw / (q * ac.Sref);		   // CD
	as.CY = *(Fw+1) / (q * ac.Sref);		   // CY
	as.CL = *(Fw+2) / (q * ac.Sref);           // CL
	as.Cl = *Mt / (q * ac.bref * ac.Sref);     // Cl
	as.Cm = *(Mt+1) / (q * ac.cref * ac.Sref); // Cm
	as.Cn = *(Mt+2) / (q * ac.bref * ac.Sref); // Cn

    free(bc); free(gam); free(winf); free(wr); free(pnl); free(we); free(wt);
	free(ui); free(vi); free(wi); free(G); free(Fp); free(Fn); free(Mn); free(dcm);
}

void Aircraft::latticeSetup(colTot &ct, geomTot &gt) {

	int N, Ns, Np[MAXGEOMSEGS], idx1, idx2, idx3, idx4, ctr;
	double *yle, *apex, *sweep, *zle, b[MAXGEOMSEGS], db[MAXGEOMSEGS-1],
			*ch, *tw, dihedral[MAXGEOMSEGS], cs[MAXGEOMSEGS], dy[MAXCOLELEMS],
			tmp1[MAXCOLELEMS],tmp2[MAXCOLELEMS],tmp3[MAXCOLELEMS], qcxf, qcyf,
			qczf;

	ctr = 0;
	for (int i = 0; i < NELEMENTS; i++) {

	    // Write out local variables of geometry parameters
		N = gt.gelem[i].N; Ns = gt.gelem[i].Ns;
		yle = gt.gelem[i].yle; zle = gt.gelem[i].zle;
		apex = gt.gelem[i].apex; sweep = gt.gelem[i].sweep; b[0] = 0;
		idx2 = 1; idx4 = 0;
		for (int j = 1; j < Ns+1; j++) {
			b[j] = sqrt(pow(yle[j-1]-apex[1],2)+pow(zle[j-1]-apex[2],2));
			db[j-1] = b[j] - b[j-1];
		}
		for (int j = 0; j < Ns; j++) {
			Np[j] = (int)round(db[j]/b[Ns]*N);
			dihedral[j] = atan((zle[j]-apex[2])/(yle[j]-apex[1]));
			ct.celem[i].dihedral[j] = dihedral[j];
			cs[j] = gt.gelem[i].cs[j];
		}
		ch = gt.gelem[i].chord; tw = gt.gelem[i].twist;

	    // Compute quarter chord x, y, z. Compute chord, twist, dihedral. Flag
	    // surfaces as control surfaces
        ct.celem[i].qc[0][0] = 0;
        ct.celem[i].qc[0][1] = 0;
        ct.celem[i].qc[0][2] = 0;
	    for (int j = 0; j < Ns; j++) {
	        idx1 = idx2;
	        idx2 = idx1 + Np[j];
	        idx3 = idx4+1;
	        idx4 = idx3+Np[j]-1;
	        for (int k = 0; k < Np[j]+1; k++) {
	        	dy[k] = db[j]/Np[j]*k;
	        }
	        qcxf = ct.celem[i].qc[idx1-1][0];
	        qcyf = ct.celem[i].qc[idx1-1][1];
	        qczf = ct.celem[i].qc[idx1-1][2];
	        for (int k = (idx1-1); k < idx2; k++) {
	        	ct.celem[i].qc[k][0] = apex[0]+qcxf-tan(sweep[j])*dy[k-(idx1-1)];
	        	ct.celem[i].qc[k][1] = apex[1]+qcyf+cos(dihedral[j])*dy[k-(idx1-1)];
	        	ct.celem[i].qc[k][2] = apex[2]+qczf+sin(dihedral[j])*dy[k-(idx1-1)];
	        	ct.celem[i].ch[k] = ch[j] + (ch[j+1]-ch[j])/db[j]*dy[k-(idx1-1)];
	        	ct.celem[i].tw[k] = tw[j] + (tw[j+1]-tw[j])/db[j]*dy[k-(idx1-1)];
	        }
	        for (int k = idx3-1; k < idx4; k++) {
	        	ct.celem[i].di[k] = -dihedral[j];
	        	ct.celem[i].cs[k] = cs[j];
	        }
	    }

	    // Mirror geometry if necessary
		if (gt.gelem[i].mirror == 1) {
			// First mirror quarter chord point
			ctr = N;
			for (int k = 0; k < N + 1; k++) {
				tmp1[k] = ct.celem[i].qc[ctr][0];
				tmp2[k] = -ct.celem[i].qc[ctr][1];
				tmp3[k] = ct.celem[i].qc[ctr][2];
				ctr--;
			}
			for (int k = 0; k < N + 1; k++) {
				ct.celem[i].qc[k][0] = tmp1[k];
				ct.celem[i].qc[k][1] = tmp2[k];
				ct.celem[i].qc[k][2] = tmp3[k];
			}
			ctr = N-1;
			for (int k = N + 1; k < 2 * N + 1; k++) {
				ct.celem[i].qc[k][0] = ct.celem[i].qc[ctr][0];
				ct.celem[i].qc[k][1] = -ct.celem[i].qc[ctr][1];
				ct.celem[i].qc[k][2] = ct.celem[i].qc[ctr][2]; ctr--;
			}
			// Mirror chord, twist
			ctr = N;
			for (int k = 0; k < N + 1; k++) {
				tmp1[k] = ct.celem[i].ch[ctr];
				tmp2[k] = ct.celem[i].tw[ctr];
				ctr--;
			}
			for (int k = 0; k < N + 1; k++) {
				ct.celem[i].ch[k] = tmp1[k];
				ct.celem[i].tw[k] = tmp2[k];
			} ctr = N-1;
			for (int k = N + 1; k < 2 * N + 1; k++) {
				ct.celem[i].ch[k] = ct.celem[i].ch[ctr];
				ct.celem[i].tw[k] = ct.celem[i].tw[ctr];  ctr--;
			}
			// Mirror dihedral, control type
			ctr = N-1;
			for (int k = 0; k < N; k++) {
				tmp1[k] = ct.celem[i].di[ctr];
				tmp2[k] = ct.celem[i].cs[ctr];
				ctr--;
			}
			for (int k = 0; k < N; k++) {
				ct.celem[i].di[k] = tmp1[k];
				ct.celem[i].cs[k] = tmp2[k];
			} ctr = N-1;
			for (int k = N; k < 2 * N; k++) {
				ct.celem[i].di[k] = -ct.celem[i].di[ctr];
				ct.celem[i].cs[k] = -ct.celem[i].cs[ctr];  ctr--;
			}
			ct.celem[i].N = 2*N;
		} else {
			ct.celem[i].N = N;
		}

		// Compute panel length
		for (int k = 0; k < 2*N; k++) {
			ct.celem[i].ds[k] = sqrt(pow(ct.celem[i].qc[k+1][1]-ct.celem[i].qc[k][1],2)
					+pow(ct.celem[i].qc[k+1][2]-ct.celem[i].qc[k][2],2));
		}

	    // Offset qc back from apex in the x-dir by 1/4 chord
		for (int k = 0; k < 2*N+1; k++) {
			ct.celem[i].qc[k][0] = ct.celem[i].qc[k][0]-ct.celem[i].ch[N]/4.0;
		}

	    // Compute trailing edge position
		for (int k = 0; k < 2*N+1; k++) {
			ct.celem[i].te[k][0] = ct.celem[i].qc[k][0] - 0.75*ct.celem[i].ch[k];
			ct.celem[i].te[k][1] = ct.celem[i].qc[k][1];
			ct.celem[i].te[k][2] = ct.celem[i].qc[k][2];
		}

	    // Compute control point x, y, z, and twist
		for (int k = 0; k < 2*N; k++) {
			ct.celem[i].cp[k][0] = 0.5*(ct.celem[i].qc[k][0]+ct.celem[i].qc[k+1][0])-
					0.5*(ct.celem[i].ch[k]+ct.celem[i].ch[k+1])/2;
			ct.celem[i].cp[k][1] = 0.5*(ct.celem[i].qc[k][1]+ct.celem[i].qc[k+1][1]);
			ct.celem[i].cp[k][2] = 0.5*(ct.celem[i].qc[k][2]+ct.celem[i].qc[k+1][2]);
			ct.celem[i].cptw[k] = 0.5*(ct.celem[i].tw[k]+ct.celem[i].tw[k+1]);
		}
	    // Compute x-location of unswept points
		for (int k = 0; k < 2*N; k++) {
			ct.celem[i].qcusx[k] = ct.celem[i].cp[k][0] + 0.5*(ct.celem[i].ch[k]+
					ct.celem[i].ch[k+1])/2;
		}

	    // Compute panel normal
		for (int k = 0; k < 2*N; k++) {
			ct.celem[i].nor[k][0] = -sin(ct.celem[i].cptw[k]);
			ct.celem[i].nor[k][1] = -sin(ct.celem[i].di[k]);
			ct.celem[i].nor[k][2] = -cos(ct.celem[i].cptw[k])*cos(ct.celem[i].di[k]);
		}
	}
}

void Aircraft::acInit(acStruct &ac, colTot &ct) {
	int k;

	// Get airfoil and mass properties
	MassPropInit(ac);
	AirfoilPropInit(ac);

	// Total parasite drag
	ac.CDp = 0.033;

	// Compute reference areas and lengths (assumed to be from the first element)
	ac.Sref = 0; ac.bref = 0;
	for (k = 0; k < ct.celem[0].N; k++) {
		ac.Sref += ct.celem[0].ds[k]*(0.5*(ct.celem[0].ch[k]+ct.celem[0].ch[k+1]));
		ac.bref = fmax(ct.celem[0].qc[k][1],ac.bref);
	}
	ac.bref = 2*ac.bref;
	double croot = ct.celem[0].ch[(int)floor((ct.celem[0].N)/2)];
	double ctip  = ct.celem[0].ch[0];
	ac.cref = (2.0/3.0)*(croot+ctip-croot*ctip/(croot+ctip));

	// Compute total number of points
	ac.Ntot = 0;
	for (int i = 0; i < NELEMENTS; i++) {
	    ac.Ntot = ac.Ntot + ct.celem[i].N;
	}

	// Compute vector from c.g. to center of quarter chord point
	k = 0;
	for (int i = 0; i < NELEMENTS; i++) {
		for (int j = 0; j < ct.celem[i].N; j++) {
			ac.macp[k][0] = ct.celem[i].cp[j][0]-ac.cg[0];
			ac.macp[k][1] = ct.celem[i].cp[j][1]-ac.cg[1];
			ac.macp[k][2] = ct.celem[i].cp[j][2]-ac.cg[2];
			ac.maqc[k][0] = ct.celem[i].qcusx[j]-ac.cg[0];
			ac.maqc[k][1] = ct.celem[i].cp[j][1]-ac.cg[1];
			ac.maqc[k][2] = ct.celem[i].cp[j][2]-ac.cg[2];
			k++;
		}
	}
	// Write panel normal array
	k = 0;
	for (int i = 0; i < NELEMENTS; i++) {
		for (int j = 0; j < ct.celem[i].N; j++) {
			ac.pnorm[k][0] = ct.celem[i].nor[j][0];
			ac.pnorm[k][1] = ct.celem[i].nor[j][1];
			ac.pnorm[k][2] = ct.celem[i].nor[j][2];
			ac.di[k] = ct.celem[i].di[j];
			ac.tw[k] = ct.celem[i].cptw[j];
			k++;
		}
	}

	//  Compute downwash
	downwash(ac, ct);

	// Compute bound vortex length and bound vortex vector
	k = 0;
	for (int i = 0; i < NELEMENTS; i++) {
	    // Compute panel length (pl)
		for (int j = 0; j < ct.celem[i].N; j++) {
			ac.plt[k] = sqrt(pow(ct.celem[i].qc[j+1][1]-
					ct.celem[i].qc[j][1],2)+
					pow(ct.celem[i].qc[j+1][2]-ct.celem[i].qc[j][2],2));
			// Project gamma along unswept bound vortex (normalized by panel length)
			ac.gvec[k][0] = 0;
			ac.gvec[k][1] = (ct.celem[i].qc[j+1][1]-
					ct.celem[i].qc[j][1])/ac.plt[k];
			ac.gvec[k][2] = (ct.celem[i].qc[j+1][2]-
					ct.celem[i].qc[j][2])/ac.plt[k];
			k++;
		}
	}

	// Store x, y, z positions of the quarter chord and center of bound vortex
	k = 0;
	for (int i = 0; i < NELEMENTS; i++) {
		for (int j = 0; j < ct.celem[i].N; j++) {
			ac.qcb[k][0] = ct.celem[i].qcusx[j];
			ac.qcb[k][1] = ct.celem[i].cp[j][1];
			ac.qcb[k][2] = ct.celem[i].cp[j][2];

			ac.cp[k][0] = ct.celem[i].cp[j][0];
			ac.cp[k][1] = ct.celem[i].cp[j][1];
			ac.cp[k][2] = ct.celem[i].cp[j][2];
	    	k++;
		}
	}

	// Store panel areas, control surface flags
	k = 0;
	for (int i = 0; i < NELEMENTS; i++) {
		for (int j = 0; j < ct.celem[i].N; j++) {
			ac.pa[k] = ct.celem[i].ds[j]*(0.5*(ct.celem[i].ch[j]+ct.celem[i].ch[j+1]));
			ac.cs[k] = ct.celem[i].cs[j];
			k++;
		}
	}

	// Store index of panel nearest to the cg. Assumed to be the midpoint of the
	// first element
	ac.cgp = (int)floor(ct.celem[0].N/2);

}

/*
 * Initializes environmental parameters
 */
void envInit(enStruct &en) {
	en.Vr = 0;
}

void Aircraft::crossprod(double a1, double a2, double a3, double b1,
		double b2, double b3, double *c) {

	*c     = a2*b3-a3*b2;
	*(c+1) = a3*b1-a1*b3;
	*(c+2) = a1*b2-a2*b1;

}

void Aircraft::controlSurfaces(int Ntot, double ail, double ele, double rud,
		double flp, double *pnorm, double *cs, double *tw, double *di) {
	/*
	*	Changes the local twist of a panel by adjusting the panel normal based on the desired contrl
	*   surface deflection. The pointer 'cs' is the control surface flag. +/-1 is aileron, +/-2 is
	*   elevator, +/-3 is rudder, +/-4 is the flap.
	*/
		int i;
		double *ptr, *ptr1, *ptr2, *ptr3;

		ptr = pnorm; ptr1 = cs; ptr2 = tw; ptr3 = di;
		for (i = 0; i < Ntot; i++) {
			if (*ptr1 == 1) {
				*ptr          = -sin(*ptr2 + ail);
				*(ptr+2*Ntot) = -cos(*ptr2 + ail)*cos(*ptr3);
			}
			else if (*ptr1 == -1) {
				*ptr          = -sin(*ptr2 - ail);
				*(ptr+2*Ntot) = -cos(*ptr2 - ail)*cos(*ptr3);
			}
			else if (*ptr1 == 2 || *ptr1 == -2) {
				*ptr          = -sin(*ptr2 + ele);
				*(ptr+2*Ntot) = -cos(*ptr2 + ele)*cos(*ptr3);
			}
			else if (*ptr1 == 3 || *ptr1 == -3) {
				*ptr          = -sin(*ptr2 + rud);
				*(ptr+2*Ntot) = -cos(*ptr2 + rud)*cos(*ptr3);
			}
			else if (*ptr1 == 4 || *ptr1 == -4) {
				*ptr          = -sin(*ptr2 + flp);
				*(ptr+2*Ntot) = -cos(*ptr2 + flp)*cos(*ptr3);
			}
			ptr++;; ptr1++; ptr2++; ptr3++;
		}
}

void Aircraft::bodyTOwind(double alpha, double beta, double *dcm) {

	double sa, sb, ca, cb;

	sa = sin(alpha);
	sb = sin(beta);
	ca = cos(alpha);
	cb = cos(beta);

	*dcm     = -cb*ca;
	*(dcm+1) = sb*ca;
	*(dcm+2) = sa;
	*(dcm+3) = sb;
	*(dcm+4) = cb;
	*(dcm+5) = 0.0;
	*(dcm+6) = -sa*cb;
	*(dcm+7) = sa*sb;
	*(dcm+8) = -ca;

}

void Aircraft::checkCLmax(int Ntot, double *dcm, double *Fn, double rho, double *wt, double *pa, double *cs,
	double CLmax, double CLmin, double flp) {

		double *Fw, *tmp, *tmp2, *q, *Cl, *CLmax_vec;
		int i;

		Fw   = (double*)malloc(sizeof(double)*Ntot*3);
		q    = (double*)malloc(sizeof(double)*Ntot);
		Cl   = (double*)malloc(sizeof(double)*Ntot);
		tmp  = (double*)malloc(sizeof(double)*3);
		tmp2 = (double*)malloc(sizeof(double)*3);
		CLmax_vec = (double*)malloc(sizeof(double)*Ntot);

		// Convert from body frame to wind frame
		for (i = 0; i < Ntot; i++) {
			*tmp     = *(Fn+i);
			*(tmp+1) = *(Fn+i+Ntot);
			*(tmp+2) = *(Fn+i+2*Ntot);
			dgemv_("n",&ithree,&ithree,&done,dcm,&ithree,tmp,&ione,&dzero,tmp2,&ione);
			*(Fw+i)        = *tmp2;
			*(Fw+i+Ntot)   = *(tmp2+1);
			*(Fw+i+2*Ntot) = *(tmp2+2);
		}

		// Compute local dynamic pressure and local lift coefficent
		for (i = 0; i < Ntot; i++) {
			*(q+i) = 0.5*rho*(pow((*(wt+i)),2)+pow((*(wt+i+Ntot)),2)+pow((*(wt+i+2*Ntot)),2));
			*(Cl+i) = *(Fw+i+2*Ntot)/(*(q+i)*(*(pa+i)));
		}

		// Allow CLmax where flaps are deployed to be higher (bit of a hack)
		for (i = 0; i < Ntot; i++) {
			if (*(cs+i) == 4 || *(cs+i) == -4) {
				*(CLmax_vec+i) = CLmax + flp;
			} else {
				*(CLmax_vec+i) = CLmax;
			}
		}

		for (i = 0; i < Ntot; i++) {
			if (*(Cl+i) > *(CLmax_vec+i)) {
				*(Cl+i) = *(CLmax_vec+i);
			}
			if (*(Cl+i) < CLmin) {
				*(Cl+i) = CLmin;
			}
			*(Fw+i+2*Ntot) = *(Cl+i)*(*(q+i))*(*(pa+i));
		}

		// Convert from wind frame to body frame
		for (i = 0; i < Ntot; i++) {
			*tmp     = *(Fw+i);
			*(tmp+1) = *(Fw+i+Ntot);
			*(tmp+2) = *(Fw+i+2*Ntot);
			dgemv_("t",&ithree,&ithree,&done,dcm,&ithree,tmp,&ione,&dzero,tmp2,&ione);
			*(Fn+i)        = *tmp2;
			*(Fn+i+Ntot)   = *(tmp2+1);
			*(Fn+i+2*Ntot) = *(tmp2+2);
		}
	free(Fw); free(tmp); free(tmp2); free(q); free(Cl); free(CLmax_vec);
}

void Aircraft::getMeasModelState(double *statein, double *stateout, double *ctrl,
		double dt, bool flag) {

	double Fg[3], Ft[3], Fp[3], accel[3], tmp[3], tmp2[3], g, pbu, qbu, rbu;
	int info, *ipiv;

	ipiv = (int*)malloc(sizeof(int)*3);
	double *dcm  = (double*)malloc(sizeof(double)*9);

	// Extract state
	double N = *statein;
	double E = *(statein+1);
	double D = *(statein+2);
	double VNi = *(statein+3);
	double VEi = *(statein+4);
	double VDi = *(statein+5);
	double q0 = *(statein+6);
	double q1 = *(statein+7);
	double q2 = *(statein+8);
	double q3 = *(statein+9);
	double pb = *(statein+10);
	double qb = *(statein+11);
	double rb = *(statein+12);
	double VNw = *(statein+13);
	double VEw = *(statein+14);
	double VDw = *(statein+15);
	double abx = *(statein+16);
	double aby = *(statein+17);
	double abz = *(statein+18);
	double wbx  = *(statein+19);
	double wby  = *(statein+20);
	double wbz  = *(statein+21);
	double barob  = *(statein+22);
	double diffb  = *(statein+23);

	// Compute Euler angles
	double phi = atan2(2*(q0*q1+q2*q3),1-2*(q1*q1+q2*q2));
	double th  = asin(2*(q0*q2-q3*q1));
	double psi = atan2(2*(q0*q3+q1*q2),1-2*(q2*q2+q3*q3));

	// Compute DCM matrix to convert body velocities to inerital velocities
	body2inertial(psi,th,phi,dcm);
	double ub = *(dcm)*VNi   + *(dcm+1)*VEi + *(dcm+2)*VDi;
	double vb = *(dcm+3)*VNi + *(dcm+4)*VEi + *(dcm+5)*VDi;
	double wb = *(dcm+6)*VNi + *(dcm+7)*VEi + *(dcm+8)*VDi;

	double ubw = *(dcm)*VNw   + *(dcm+1)*VEw + *(dcm+2)*VDw;
	double vbw = *(dcm+3)*VNw + *(dcm+4)*VEw + *(dcm+5)*VDw;
	double wbw = *(dcm+6)*VNw + *(dcm+7)*VEw + *(dcm+8)*VDw;

	// Compute airspeed
	double V = sqrt((ub-ubw)*(ub-ubw)+(vb-vbw)*(vb-vbw)+(wb-wbw)*(wb-wbw));

	// Compute force due to gravity
	g = 9.81; //atm->getGravity(D);
	Fg[0] = -sin(th)*ac.mass*g;
	Fg[1] = sin(phi)*cos(th)*ac.mass*g;
	Fg[2] = cos(phi)*cos(th)*ac.mass*g;

	if (V > 3) {
		// Compute aerodynamic forces and moments
		aeroSolverLinear(statein,ctrl);

		// Compute propulsive thrust
		Fp[0] = propulsion->getThrust(V,D,cv.thrPercent);
		Fp[1] = 0;
		Fp[2] = 0;

		// Compute propulsive torque
		as.M[0]+= propulsion->getTorque(V,D,cv.thrPercent);

		// Sum total forces
		for (int i = 0; i < 3; i++) {
			Ft[i] = as.F[i] + Fg[i] + Fp[i];
		}

		// Compute accelerations in the body frame: a_b = F/m - (w x V)
		tmp[0] = vb*rb-wb*qb;
		tmp[1] = wb*pb-ub*rb;
		tmp[2] = ub*qb-vb*pb;
		for (int i = 0; i < 3; i++) {
			accel[i] = Ft[i]/ac.mass + tmp[i];
		}

		// Compute rotational acceleration wdot: M_b = I*wdot + w x (I*w)
		tmp[0] = *(ac.I)*pb   + *(ac.I+3)*qb + *(ac.I+6)*rb;
		tmp[1] = *(ac.I+1)*pb + *(ac.I+4)*qb + *(ac.I+7)*rb;
		tmp[2] = *(ac.I+2)*pb + *(ac.I+5)*qb + *(ac.I+8)*rb;
		tmp2[0] = qb*tmp[2]-rb*tmp[1];
		tmp2[1] = rb*tmp[0]-pb*tmp[2];
		tmp2[2] = pb*tmp[1]-qb*tmp[0];
		tmp[0] = as.M[0]-tmp2[0];
		tmp[1] = as.M[1]-tmp2[1];
		tmp[2] = as.M[2]-tmp2[2];

		// solve system of linear equations (writes wdot into tmp)
		dgesv_(&ithree, &ione, ac.I, &ithree, ipiv, tmp, &ithree, &info);

		// Estimate measurements
		pbu  = pb + tmp[0]*dt;
		qbu  = qb + tmp[1]*dt;
		rbu  = rb + tmp[2]*dt;

	} else {
		accel[0] = 0;
		accel[1] = 0;
		accel[2] = 0;
		pbu = 0;
		qbu = 0;
		rbu = 0;
	}

	// Write 'derivative' array
	if (flag) { // IMU
		*(stateout+0) = accel[0] + Fg[0]/ac.mass - abx;
		*(stateout+1) = accel[1] + Fg[1]/ac.mass - aby;
		*(stateout+2) = accel[2] + Fg[2]/ac.mass - abz;
		*(stateout+3) = pbu - wbx;
		*(stateout+4) = qbu - wby;
		*(stateout+5) = rbu - wbz;
		*(stateout+6) = 101325.0*pow((1.0+0.0000225721785*D),5.256)-barob;
		*(stateout+7) = 0.6125*((VNi-VNw)*(VNi-VNw)+(VEi-VEw)*(VEi-VEw)+
				(VDi-VDw)*(VDi-VDw))-diffb;
	} else { // IMU & GPS
		*(stateout+0) = accel[0] + Fg[0]/ac.mass - abx;
		*(stateout+1) = accel[1] + Fg[1]/ac.mass - aby;
		*(stateout+2) = accel[2] + Fg[2]/ac.mass - abz;
		*(stateout+3) = pbu - wbx;
		*(stateout+4) = qbu - wby;
		*(stateout+5) = rbu - wbz;
		*(stateout+6) = 101325.0*pow((1.0+0.0000225721785*D),5.256)-barob;
		*(stateout+7) = 0.6125*((VNi-VNw)*(VNi-VNw)+(VEi-VEw)*(VEi-VEw)+
				(VDi-VDw)*(VDi-VDw))-diffb;
		*(stateout+8) = N;
		*(stateout+9) = E;
		*(stateout+10) = D;
		*(stateout+11) = VNi;
		*(stateout+12) = VEi;
	}
	free(ipiv); free(dcm);
}

void Aircraft::getProcessModelState(double *state, double *derivs, double *ctrl,
		double dt) {

	double Fg[3], Ft[3], Fp[3], accel[3], tmp[3], tmp2[3], phidot, thdot, psidot,
		   Veu[3], *dcm, g;
	int info, *ipiv;

	ipiv = (int*)malloc(sizeof(int)*3);
	dcm  = (double*)malloc(sizeof(double)*9);

	// Extract state
	double Np = *(state+0);
	double Ep = *(state+1);
	double Dp = *(state+2);
	double VNi = *(state+3);
	double VEi = *(state+4);
	double VDi = *(state+5);
	double q0 = *(state+6);
	double q1 = *(state+7);
	double q2 = *(state+8);
	double q3 = *(state+9);
	double pb = *(state+10);
	double qb = *(state+11);
	double rb = *(state+12);
	double VNw = *(state+13);
	double VEw = *(state+14);
	double VDw = *(state+15);

	// Compute Euler angles
	double phi = atan2(2*(q0*q1+q2*q3),1-2*(q1*q1+q2*q2));
	double th  = asin(2*(q0*q2-q3*q1));
	double psi = atan2(2*(q0*q3+q1*q2),1-2*(q2*q2+q3*q3));

	// Compute DCM matrix to convert body velocities to inerital velocities
	body2inertial(psi,th,phi,dcm);
	double ub = *(dcm)*VNi   + *(dcm+1)*VEi + *(dcm+2)*VDi;
	double vb = *(dcm+3)*VNi + *(dcm+4)*VEi + *(dcm+5)*VDi;
	double wb = *(dcm+6)*VNi + *(dcm+7)*VEi + *(dcm+8)*VDi;

	double ubw = *(dcm)*VNw   + *(dcm+1)*VEw + *(dcm+2)*VDw;
	double vbw = *(dcm+3)*VNw + *(dcm+4)*VEw + *(dcm+5)*VDw;
	double wbw = *(dcm+6)*VNw + *(dcm+7)*VEw + *(dcm+8)*VDw;

	// Compute airspeed
	double V = sqrt((ub-ubw)*(ub-ubw)+(vb-vbw)*(vb-vbw)+(wb-wbw)*(wb-wbw));

	// Compute force due to gravity
	g = 9.81; //atm->getGravity(D);
	Fg[0] = -sin(th)*ac.mass*g;
	Fg[1] = sin(phi)*cos(th)*ac.mass*g;
	Fg[2] = cos(phi)*cos(th)*ac.mass*g;

	// Make sure airspeed exceeds threshold
	if (V > 3) {
		// Compute aerodynamic forces and moments
		aeroSolverLinear(state,ctrl);

		// Compute propulsive thrust
		Fp[0] = propulsion->getThrust(V,-Dp,cv.thrPercent);
		Fp[1] = 0;
		Fp[2] = 0;

		// Compute propulsive torque
		as.M[0]+=propulsion->getTorque(V,-Dp,cv.thrPercent);

		// Sum total forces
		for (int i = 0; i < 3; i++) {
			Ft[i] = as.F[i] + Fg[i] + Fp[i];
		}

		// Compute accelerations in the body frame: a_b = F/m - (w x V)
		tmp[0] = vb*rb-wb*qb;
		tmp[1] = wb*pb-ub*rb;
		tmp[2] = ub*qb-vb*pb;
		for (int i = 0; i < 3; i++) {
			accel[i] = Ft[i]/ac.mass + tmp[i];
		}

		// Compute rotational acceleration wdot: M_b = I*wdot + w x (I*w)
		tmp[0] = *(ac.I)*pb   + *(ac.I+3)*qb + *(ac.I+6)*rb;
		tmp[1] = *(ac.I+1)*pb + *(ac.I+4)*qb + *(ac.I+7)*rb;
		tmp[2] = *(ac.I+2)*pb + *(ac.I+5)*qb + *(ac.I+8)*rb;
		tmp2[0] = qb*tmp[2]-rb*tmp[1];
		tmp2[1] = rb*tmp[0]-pb*tmp[2];
		tmp2[2] = pb*tmp[1]-qb*tmp[0];
		tmp[0] = as.M[0]-tmp2[0];
		tmp[1] = as.M[1]-tmp2[1];
		tmp[2] = as.M[2]-tmp2[2];

		// solve system of linear equations (writes wdot into tmp)
		dgesv_(&ithree, &ione, ac.I, &ithree, ipiv, tmp, &ithree, &info);

		// Calculate Euler angle rates
		phidot = pb+(qb*sin(phi)+rb*cos(phi))*(sin(th)/cos(th));
		thdot  = qb*cos(phi)-rb*sin(phi);
		psidot = (qb*sin(phi)+rb*cos(phi))/cos(th);

		double ubu = ub + accel[0]*dt;
		double vbu = ub + accel[1]*dt;
		double wbu = ub + accel[2]*dt;

		// Compute DCM matrix to convert body velocities to inerital velocities
		body2inertial(psi,th,phi,dcm);
		Veu[0] = *(dcm)*ubu   + *(dcm+3)*vbu + *(dcm+6)*wbu;
		Veu[1] = *(dcm+1)*ubu + *(dcm+4)*vbu + *(dcm+7)*wbu;
		Veu[2] = *(dcm+2)*ubu + *(dcm+5)*vbu + *(dcm+8)*wbu;

		// Estimate new positions based on inertial velocities
		double Npu  = Np + VNi*dt;
		double Epu  = Ep + VEi*dt;
		double Dpu  = Dp + VDi*dt;
		double phiu = phi + phidot*dt;
		double thu  = th  + thdot*dt;
		double psiu = psi + psidot*dt;

		double q0u  = cos(phiu/2.0)*cos(thu/2.0)*cos(psiu/2.0)+
					  sin(phiu/2.0)*sin(thu/2.0)*sin(psiu/2.0);
		double q1u  = sin(phiu/2.0)*cos(thu/2.0)*cos(psiu/2.0)-
					  cos(phiu/2.0)*sin(thu/2.0)*sin(psiu/2.0);
		double q2u  = cos(phiu/2.0)*sin(thu/2.0)*cos(psiu/2.0)+
					  sin(phiu/2.0)*cos(thu/2.0)*sin(psiu/2.0);
		double q3u  = cos(phiu/2.0)*cos(thu/2.0)*sin(psiu/2.0)-
					  sin(phiu/2.0)*sin(thu/2.0)*cos(psiu/2.0);
		double pbu  = pb + tmp[0]*dt;
		double qbu  = qb + tmp[1]*dt;
		double rbu  = rb + tmp[2]*dt;

		// Write 'derivative' array
		*(derivs+0) = Npu;
		*(derivs+1) = Epu;
		*(derivs+2) = Dpu;
		*(derivs+3) = Veu[0];
		*(derivs+4) = Veu[1];
		*(derivs+5) = Veu[2];
		*(derivs+6) = q0u;
		*(derivs+7) = q1u;
		*(derivs+8) = q2u;
		*(derivs+9) = q3u;
		*(derivs+10) = pbu;
		*(derivs+11) = qbu;
		*(derivs+12) = rbu;
		*(derivs+13) = *(state+13);   // VNi_wind
		*(derivs+14) = *(state+14);   // VEi_wind
		*(derivs+15) = *(state+15);   // VDi_wind
		*(derivs+16) = *(state+16);   // abx
		*(derivs+17) = *(state+17);   // aby
		*(derivs+18) = *(state+18);   // abz
		*(derivs+19) = *(state+19);   // wbx
		*(derivs+20) = *(state+20);   // wby
		*(derivs+21) = *(state+21);   // wbz
		*(derivs+22) = *(state+22);   // barob
		*(derivs+23) = *(state+23);   // diffb
	} else {
		*(derivs+0) = Np;
		*(derivs+1) = Ep;
		*(derivs+2) = Dp;
		*(derivs+3) = 0;
		*(derivs+4) = 0;
		*(derivs+5) = 0;
		*(derivs+6) = q0;
		*(derivs+7) = q1;
		*(derivs+8) = q2;
		*(derivs+9) = q3;
		*(derivs+10) = 0;
		*(derivs+11) = 0;
		*(derivs+12) = 0;
		*(derivs+13) = *(state+13);   // VNi_wind
		*(derivs+14) = *(state+14);   // VEi_wind
		*(derivs+15) = *(state+15);   // VDi_wind
		*(derivs+16) = *(state+16);   // abx
		*(derivs+17) = *(state+17);   // aby
		*(derivs+18) = *(state+18);   // abz
		*(derivs+19) = *(state+19);   // wbx
		*(derivs+20) = *(state+20);   // wby
		*(derivs+21) = *(state+21);   // wbz
		*(derivs+22) = *(state+22);   // barob
		*(derivs+23) = *(state+23);   // diffb
	}

	free(ipiv); free(dcm);
}

//  Creates the direction cosine matrix that converts from NED body axes to
//  an inertial frame
void Aircraft::body2inertial(double psi,double th, double phi, double *dcm) {

	double spsi = sin(psi); double cpsi = cos(psi);
	double sth  = sin(th);  double cth  = cos(th);
	double sphi = sin(phi); double cphi = cos(phi);

	*dcm     = cth*cpsi;
	*(dcm+1) = cth*spsi;
	*(dcm+2) = -sth;
	*(dcm+3) = sphi*sth*cpsi - cphi*spsi;
	*(dcm+4) = sphi*sth*spsi + cphi*cpsi;
	*(dcm+5) = sphi*cth;
	*(dcm+6) = cphi*sth*cpsi + sphi*spsi;
	*(dcm+7) = cphi*sth*spsi - sphi*cpsi;
	*(dcm+8) = cphi*cth;
}

void Aircraft::envwinds(double *x, double *dcm, double *we) {

	int M = ac.Ntot;

//	double *ct = (double*)malloc(sizeof(double)*ac.Ntot*3);
//	double cgnew[3];
//
//	// Extract state
//	double N = *x;
//	double E = *(x+1);
//	double D = *(x+2);
	double VNw = *(x+13);
	double VEw = *(x+14);
	double VDw = *(x+15);
//
//    // Put 3/4 chord points in the inertial frame
////    ct = dcm*(ac.cp-tmp)';
//	for (int i = 0; i < M; i++) {
//		*(ct+i)    = *(dcm)*(ac.cp[i][0]-ac.cg[0]) +
//						 *(dcm+3)*(ac.cp[i][1]-ac.cg[1]) +
//						 *(dcm+6)*(ac.cp[i][2]-ac.cg[2]);
//		*(ct+i+M)  = *(dcm+1)*(ac.cp[i][0]-ac.cg[0]) +
//				 	 	 *(dcm+4)*(ac.cp[i][1]-ac.cg[1]) +
//				 	 	 *(dcm+7)*(ac.cp[i][2]-ac.cg[2]);
//		*(ct+i+2*M) = *(dcm+2)*(ac.cp[i][0]-ac.cg[0]) +
//				 	 	 *(dcm+5)*(ac.cp[i][1]-ac.cg[1]) +
//				 	 	 *(dcm+8)*(ac.cp[i][2]-ac.cg[2]);
//	}
//
//	// Compute new c.g. location
//	cgnew[0] = *(dcm)*ac.cg[0]   + *(dcm+3)*ac.cg[1] + *(dcm+6)*ac.cg[2];
//	cgnew[1] = *(dcm+1)*ac.cg[0] + *(dcm+4)*ac.cg[1] + *(dcm+7)*ac.cg[2];
//	cgnew[2] = *(dcm+2)*ac.cg[0] + *(dcm+5)*ac.cg[1] + *(dcm+8)*ac.cg[2];
//
//    // Add back the c.g. offset, inertial c.g. location
//	for (int i = 0; i < M; i++) {
//		*(ct+i)     += cgnew[0] + N;
//		*(ct+i+M)   += cgnew[1] + E;
//		*(ct+i+2*M) += cgnew[2] + D;
//		cout << *(ct+i) << " " << *(ct+i+M) << " " << *(ct+i+2*M) << endl;
//	}
//
//	free(ct);

//	 Add steady winds, transform to body frame
	for (int i = 0; i < M; i++) {
		*(we+i)     = *(dcm)*VNw   + *(dcm+1)*VEw + *(dcm+2)*VDw;
		*(we+i+M)   = *(dcm+3)*VNw + *(dcm+4)*VEw + *(dcm+5)*VDw;
		*(we+i+2*M)	= *(dcm+6)*VNw + *(dcm+7)*VEw + *(dcm+8)*VDw;
	}

}

void Aircraft::loadSDMatrix(){
	ifstream Sfile ("SD.txt");
	int i = 0, j = 0;
	if (Sfile.is_open()) {

		while (! Sfile.eof() ){
			Sfile >> *(SD+i+j*5);
			j++;
			if (j == 9){
				i++;
				j = 0;
			}
		}
		Sfile.close();
		cout << "Stability data loaded" << endl;
	}
}

void Aircraft::aeroSolverLinear(double *state, double *ctrl) {

	double *FM = (double*)malloc(sizeof(double)*6);
	double *tmp = (double*)malloc(sizeof(double)*9);
	double *dcm = (double*)malloc(sizeof(double)*9);
	double Fs[3];

	// Get state variables
	double Dp  = *(state+2);
	double VNi = *(state+3);
	double VEi = *(state+4);
	double VDi = *(state+5);
	double q0 = *(state+6);
	double q1 = *(state+7);
	double q2 = *(state+8);
	double q3 = *(state+9);
	double pb = *(state+10);
	double qb = *(state+11);
	double rb = *(state+12);
	double VNw = *(state+13);
	double VEw = *(state+14);
	double VDw = *(state+15);

	// Get control inputs
	double ail = *(ctrl+0);
	double ele = *(ctrl+1);
	double rud = *(ctrl+2);

	// Compute Euler angles
	double phi = atan2(2*(q0*q1+q2*q3),1-2*(q1*q1+q2*q2));
	double th  = asin(2*(q0*q2-q3*q1));
	double psi = atan2(2*(q0*q3+q1*q2),1-2*(q2*q2+q3*q3));

	// Compute DCM matrix to convert inertial velocities to body velocities
	body2inertial(psi,th,phi,dcm);
	double ub = *(dcm)*VNi   + *(dcm+1)*VEi + *(dcm+2)*VDi;
	double vb = *(dcm+3)*VNi + *(dcm+4)*VEi + *(dcm+5)*VDi;
	double wb = *(dcm+6)*VNi + *(dcm+7)*VEi + *(dcm+8)*VDi;

	double ubw = *(dcm)*VNw   + *(dcm+1)*VEw + *(dcm+2)*VDw;
	double vbw = *(dcm+3)*VNw + *(dcm+4)*VEw + *(dcm+5)*VDw;
	double wbw = *(dcm+6)*VNw + *(dcm+7)*VEw + *(dcm+8)*VDw;

	// Compute V
	double V = sqrt(pow(ub-ubw,2)+pow(vb-vbw,2)+pow(wb-wbw,2));
	double alpha = atan((wb-wbw)/(ub-ubw));
	double beta = asin((vb-vbw)/V);

	// Compute dynamic pressure
	double rho = atm->getDensity(Dp);
	double q = 0.5*rho*pow(V,2);

	// Write vector to multiply stability derivatives by
	*(tmp+0) = alpha;
	*(tmp+1) = beta;
	*(tmp+3) = pb;
	*(tmp+4) = qb;
	*(tmp+5) = rb;
	*(tmp+6) = ail;
	*(tmp+7) = ele;
	*(tmp+8) = rud;

//	cout << alpha << " " << beta << endl;
//	cout << pb << " " << qb << " " << rb << endl;
//	cout << ail << " " << ele << " " << rud << endl;

	dgemv_("n",&ifive,&ieight,&done,SD,&ifive,tmp,&ione,&dzero,FM,&ione);

	double Sref = 0.3283;
	double cref = 0.20;
	double bref = 1.87;

	double CL0 = 0.51849;
	double Cm0 = -0.00238;
	double CL, CD0, CD1, CD2, CD3, CD4;
	CL = (*(FM+0)+CL0);

	CD4 = 1.6779;
	CD3 = -1.505;
	CD2 = 0.43374;
	CD1 = -0.008792;
	CD0 = .013368;

	// Store forces
	Fs[0] =  (CD0 + CD1*CL + CD2*CL*CL + CD3*CL*CL*CL + CD4*CL*CL*CL*CL) * (q * Sref);

	Fs[1] =  *(FM+1) * (q * Sref); // CY
	Fs[2] =  CL * (q * Sref); // CL

	as.F[0] = -1.0*(cos(alpha)*Fs[0] - sin(alpha)*Fs[2]); // Fx, Negative sign for NED
	as.F[1] = Fs[1];									  // Fy
	as.F[2] = -1.0*(sin(alpha)*Fs[0] + cos(alpha)*Fs[2]); // Fz, Negative sign for NED
	// Store moments
	as.M[0] = -*(FM+2) * (q * bref * Sref); // Roll moment, Negative sign for NED
	as.M[1] =  (*(FM+3)+Cm0) * (q * cref * Sref); // Pitch moment
	as.M[2] = -*(FM+4) * (q * bref * Sref); // Yaw moment, Negative sign for NED

	free(FM); free(tmp); free(dcm);
}
